/**
  ******************************************************************************
  * File Name          : jy901b_driver.cpp
  * Description       : jy901b_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <jy901b_driver/jy901b_driver.h>

std::string _port;
serial::Serial ser;

float degree2Rad = 3.1415926/180.0;
unsigned char r_buf[1024];
float a[3],w[3],angle[3],h[3];

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  
  * @param  
  * @retval 
  */
void ParseData(char chr)
{
    static char chrBuf[100];
    static unsigned char chrCnt=0;
    signed short sData[4];
    unsigned char i;
    
    time_t now;
    chrBuf[chrCnt++]=chr;
    if (chrCnt<11) return;
    
    if ((chrBuf[0]!=0x55)||((chrBuf[1]&0x50)!=0x50)) 
    {
      //printf("Error:%x %x\r\n",chrBuf[0],chrBuf[1]);
      memcpy(&chrBuf[0],&chrBuf[1],10);
      chrCnt--;
      return;
      }
    
    memcpy(&sData[0],&chrBuf[2],8);
	switch(chrBuf[1])
	{
	    case 0x51://线加速度
		    for (i=0;i<3;i++)
            {
                a[i] = (float)sData[i]/32768.0*16.0;
            } 
			time(&now);
		    //printf("\r\nT:%s a:%6.3f %6.3f %6.3f ",asctime(localtime(&now)),a[0],a[1],a[2]);
			break;
		case 0x52://角速度
		    for (i=0;i<3;i++)
            {
                w[i] = (float)sData[i]/32768.0*2000.0;
            } 
			//printf("w:%7.3f %7.3f %7.3f ",w[0],w[1],w[2]);					
			break;
		case 0x53://角度
		    for (i=0;i<3;i++) 
            {
                angle[i] = (float)sData[i]/32768.0*180.0;
            }
			//printf("A:%7.3f %7.3f %7.3f ",angle[0],angle[1],angle[2]);
			break;
		case 0x54://磁场
			for (i=0;i<3;i++)
            {
                h[i] = (float)sData[i];
            }
		    //printf("h:%4.0f %4.0f %4.0f ",h[0],h[1],h[2]);
			break;
		}		
		chrCnt=0;		
}

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "imu_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(10);   //10hz
    sensor_msgs::Imu imu_data;
    unsigned char  data_size;
    geometry_msgs::Quaternion q;

    nh.getParam("/jy901b_driver/port", _port);
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu_data", 1000);

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(9600); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    }

    while(ros::ok())
    {
      if(data_size = ser.available()){
        ser.read(r_buf, data_size);
        for (int i=0;i<data_size;i++) {
          //fprintf(fp,"%2X ",r_buf[i]);
          ParseData(r_buf[i]);
        }
      }

        imu_data.header.stamp = ros::Time::now();
        imu_data.header.frame_id = "imu_link";

        q = tf::createQuaternionMsgFromRollPitchYaw(angle[0]*degree2Rad, angle[1]*degree2Rad, angle[2]*degree2Rad);

        imu_data.orientation.x = q.x;
        imu_data.orientation.y = q.y;
        imu_data.orientation.z = q.z;
        imu_data.orientation.w = q.w;
       
        imu_data.angular_velocity.x = w[0];
        imu_data.angular_velocity.y = w[1];
        imu_data.angular_velocity.z = w[2];
 
        imu_data.linear_acceleration.x = a[0];
        imu_data.linear_acceleration.y = a[1];
        imu_data.linear_acceleration.z = a[2];
        imu_data.orientation_covariance = {
                    0.05, 0, 0,
                    0, 0.05, 0,
                    0, 0, 0.05
                  };
        imu_data.angular_velocity_covariance = {
                    0.01, 0, 0,
                    0, 0.01, 0,
                    0, 0, 0.01
                  };
        imu_data.angular_velocity_covariance = {
                    -1, 0, 0,
                    0, 0, 0,
                    0, 0, 0
                  };



        imu_pub.publish(imu_data);
        ros::spinOnce();
        loop_rate.sleep();
    }

    ser.close(); 
    return 0;
}
